#include "hal_mpu6050.h"

#include "i2c.h"
#include "main.h"

// 初始化MPU6050
// 返回值:0,成功
//     其他,错误代码
uint8_t HAL_MPU_Init(void) {
  uint8_t res;

  HAL_MPU_Write_Byte(HAL_MPU_PWR_MGMT1_REG, 0X80);  // 复位MPU6050
  HAL_Delay(100);
  HAL_MPU_Write_Byte(HAL_MPU_PWR_MGMT1_REG, 0X00);  // 唤醒MPU6050
  HAL_MPU_Set_Gyro_Fsr(3);                          // 陀螺仪传感器,±2000dps
  HAL_MPU_Set_Accel_Fsr(0);                         // 加速度传感器,±2g
  HAL_MPU_Set_Rate(50);                             // 设置采样率50Hz
  HAL_MPU_Write_Byte(HAL_MPU_INT_EN_REG, 0X00);     // 关闭所有中断
  HAL_MPU_Write_Byte(HAL_MPU_USER_CTRL_REG, 0X00);  // I2C主模式关闭
  HAL_MPU_Write_Byte(HAL_MPU_FIFO_EN_REG, 0X00);    // 关闭FIFO
  HAL_MPU_Write_Byte(HAL_MPU_INTBP_CFG_REG, 0X80);  // INT引脚低电平有效
  res = HAL_MPU_Read_Byte(HAL_MPU_DEVICE_ID_REG);
  if (res == HAL_MPU_ADDR)  // 器件ID正确
  {
    HAL_MPU_Write_Byte(HAL_MPU_PWR_MGMT1_REG,
                       0X01);  // 设置CLKSEL,PLL X轴为参考
    HAL_MPU_Write_Byte(HAL_MPU_PWR_MGMT2_REG, 0X00);  // 加速度与陀螺仪都工作
    HAL_MPU_Set_Rate(50);                             // 设置采样率为50Hz
  } else
    return 1;
  return 0;
}
// 设置MPU6050陀螺仪传感器满量程范围
// fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
// 返回值:0,设置成功
//     其他,设置失败
uint8_t HAL_MPU_Set_Gyro_Fsr(uint8_t fsr) {
  return HAL_MPU_Write_Byte(HAL_MPU_GYRO_CFG_REG,
                            fsr << 3);  // 设置陀螺仪满量程范围
}
// 设置MPU6050加速度传感器满量程范围
// fsr:0,±2g;1,±4g;2,±8g;3,±16g
// 返回值:0,设置成功
//     其他,设置失败
uint8_t HAL_MPU_Set_Accel_Fsr(uint8_t fsr) {
  return HAL_MPU_Write_Byte(HAL_MPU_ACCEL_CFG_REG,
                            fsr << 3);  // 设置加速度传感器满量程范围
}
// 设置MPU6050的数字低通滤波器
// lpf:数字低通滤波频率(Hz)
// 返回值:0,设置成功
//     其他,设置失败
uint8_t HAL_MPU_Set_LPF(uint16_t lpf) {
  uint8_t data = 0;
  if (lpf >= 188)
    data = 1;
  else if (lpf >= 98)
    data = 2;
  else if (lpf >= 42)
    data = 3;
  else if (lpf >= 20)
    data = 4;
  else if (lpf >= 10)
    data = 5;
  else
    data = 6;
  return HAL_MPU_Write_Byte(HAL_MPU_CFG_REG, data);  // 设置数字低通滤波器
}
// 设置MPU6050的采样率(假定Fs=1KHz)
// rate:4~1000(Hz)
// 返回值:0,设置成功
//     其他,设置失败
uint8_t HAL_MPU_Set_Rate(uint16_t rate) {
  uint8_t data;
  if (rate > 1000) rate = 1000;
  if (rate < 4) rate = 4;
  data = 1000 / rate - 1;
  data =
      HAL_MPU_Write_Byte(HAL_MPU_SAMPLE_RATE_REG, data);  // 设置数字低通滤波器
  return HAL_MPU_Set_LPF(rate / 2);  // 自动设置LPF为采样率的一半
}

// 得到温度值
// 返回值:温度值(扩大了100倍)
short HAL_MPU_Get_Temperature(void) {
  uint8_t buf[2];
  short raw;
  float temp;
  HAL_MPU_Read_Len(HAL_MPU_TEMP_OUTH_REG, 2, buf);
  raw = ((uint16_t)buf[0] << 8) | buf[1];
  temp = 36.53 + ((double)raw) / 340;
  return temp * 100;
  ;
}
// 得到陀螺仪值(原始值)
// gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
// 返回值:0,成功
//     其他,错误代码
uint8_t HAL_MPU_Get_Gyroscope(short *gx, short *gy, short *gz) {
  uint8_t buf[6], res;
  res = HAL_MPU_Read_Len(HAL_MPU_GYRO_XOUTH_REG, 6, buf);
  if (res == 0) {
    *gx = ((uint16_t)buf[0] << 8) | buf[1];
    *gy = ((uint16_t)buf[2] << 8) | buf[3];
    *gz = ((uint16_t)buf[4] << 8) | buf[5];
  }
  return res;
  ;
}
// 得到加速度值(原始值)
// gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
// 返回值:0,成功
//     其他,错误代码
uint8_t HAL_MPU_Get_Accelerometer(short *ax, short *ay, short *az) {
  uint8_t buf[6], res;
  res = HAL_MPU_Read_Len(HAL_MPU_ACCEL_XOUTH_REG, 6, buf);
  if (res == 0) {
    *ax = ((uint16_t)buf[0] << 8) | buf[1];
    *ay = ((uint16_t)buf[2] << 8) | buf[3];
    *az = ((uint16_t)buf[4] << 8) | buf[5];
  }
  return res;
  ;
}

// 以下为移植修改

uint8_t DMP_Write_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) {
  extern I2C_HandleTypeDef hi2c2;
  HAL_I2C_Mem_Write(&hi2c2, addr, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
  HAL_Delay(1);

  return 0;
}

uint8_t DMP_Read_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) {
  extern I2C_HandleTypeDef hi2c2;
  HAL_I2C_Mem_Read(&hi2c2, addr, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
  HAL_Delay(1);

  return 0;
}

uint8_t HAL_MPU_Write_Len(uint8_t reg, uint8_t len, uint8_t *buf) {
  extern I2C_HandleTypeDef hi2c2;
  HAL_I2C_Mem_Write(&hi2c2, HAL_MPU_WRITE, reg, I2C_MEMADD_SIZE_8BIT, buf, len,
                    0xfff);
  HAL_Delay(1);

  return 0;
}

uint8_t HAL_MPU_Read_Len(uint8_t reg, uint8_t len, uint8_t *buf) {
  extern I2C_HandleTypeDef hi2c2;
  HAL_I2C_Mem_Read(&hi2c2, HAL_MPU_READ, reg, I2C_MEMADD_SIZE_8BIT, buf, len,
                   0xfff);
  HAL_Delay(1);

  return 0;
}

uint8_t HAL_MPU_Write_Byte(uint8_t reg, uint8_t data) {
  extern I2C_HandleTypeDef hi2c2;
  unsigned char W_Data = 0;

  W_Data = data;
  HAL_I2C_Mem_Write(&hi2c2, HAL_MPU_WRITE, reg, I2C_MEMADD_SIZE_8BIT, &W_Data,
                    1, 0xfff);
  HAL_Delay(1);

  return 0;
}

uint8_t HAL_MPU_Read_Byte(uint8_t reg) {
  extern I2C_HandleTypeDef hi2c2;
  unsigned char R_Data = 0;

  HAL_I2C_Mem_Read(&hi2c2, HAL_MPU_READ, reg, I2C_MEMADD_SIZE_8BIT, &R_Data, 1,
                   0xfff);
  HAL_Delay(1);

  return R_Data;
}
